﻿using System;
using System.Collections.Generic;
using System.Threading;

namespace AStar
{
    /// <summary>
    /// Event arguments describing the node involved in the navigation event
    /// </summary>
    public class NodeNavigationEventArgs : EventArgs
    {
        /// <summary>
        /// Event involved node
        /// </summary>
        public Node Node
        {
            get;
            private set;
        }

        /// <summary>
        /// Total number of the nodes in the path
        /// </summary>
        public int Count
        {
            get;
            private set;
        }

        /// <summary>
        /// Index for the event involved node
        /// </summary>
        public int Index
        {
            get;
            private set;
        }

        /// <summary>
        /// Create a new istance of NodeNavigationEventArgs used to store the event involved node data
        /// </summary>
        /// <param name="node">Event involved node</param>
        /// <param name="index">Index of the event involved node</param>
        public NodeNavigationEventArgs(Node node, int count, int index)
        {
            this.Node = node;
            this.Count = count;
            this.Index = index;
        }
    }

    /// <summary>
    /// Delegate for path found event on async finding
    /// </summary>
    /// <param name="path"></param>
    public delegate void PathFoundEventHandler(Node[] path);

    /// <summary>
    /// Delegate for navigation events
    /// </summary>
    /// <param name="sender">Object that raised the event</param>
    /// <param name="e">Event involved node info</param>
    public delegate void NodeNavigationEventHandler(object sender, NodeNavigationEventArgs e);

    /// <summary>
    /// Define a generalized method to allow use of A* algorithm on a data structure
    /// </summary>
    public interface INodeMap
    {
        Node GetStartingNode();
        Node GetDestinationNode();
        Node[] GetChildNodes(Node actualNode);
    }

    /// <summary>
    /// Implement data do manage the nodes for the A* algorithm
    /// </summary>
    public class Node : IComparable<Node>, IEquatable<Node>
    {
        #region Properties

        /// <summary>
        /// Unique identifier for this node
        /// </summary>
        public readonly Guid NodeGuid;

        /// <summary>
        /// Get the node used during the search to record the parent of successor nodes
        /// </summary>
        public readonly Node Parent;

        /// <summary>
        /// Get cost of this node + it's predecessors
        /// </summary>
        public readonly int G_CurrentCost;

        /// <summary>
        /// Get the heuristic estimate of distance to goal
        /// </summary>
        public readonly int H_HeuristicDistance;

        /// <summary>
        /// Get the sum of cumulative cost of predecessors and self and heuristic
        /// </summary>
        public readonly int F_PathScore;

        /// <summary>
        /// Get or set the object linked to this node
        /// </summary>
        public object Item;

        #endregion

        #region Constructors

        /// <summary>
        /// Create a new instance of a node
        /// </summary>
        /// <param name="parent">Parent node</param>
        /// <param name="movementCost">Movement cost from parent node</param>
        /// <param name="heuristic">Heurustic distance from the destination node</param>
        public Node(Node parent, int movementCost, int heuristic):
            this(Guid.NewGuid(), parent, movementCost, heuristic)
        {
		}

        /// <summary>
        /// Create a new instance of a node
        /// </summary>
        /// <param name="nodeGuid">Guid of this node</param>
        /// <param name="parent">Parent node</param>
        /// <param name="movementCost">Movement cost from parent node</param>
        /// <param name="heuristic">Heurustic distance from the destination node</param>
        public Node(Guid nodeGuid, Node parent, int movementCost, int heuristic)
        {
            int ParentCurrentCost = parent == null ? 0 : parent.G_CurrentCost;

            this.NodeGuid = nodeGuid;
            this.Parent = parent;
            this.G_CurrentCost = ParentCurrentCost + movementCost;
            this.H_HeuristicDistance = heuristic;
            this.F_PathScore = this.G_CurrentCost + this.H_HeuristicDistance;
            this.Item = null;
        }

        #endregion

        #region IComparable<Node> Members

        /// <summary>
        /// Compare first the path score then the current cost of this node with another one
        /// </summary>
        /// <param name="other">Other node to compare</param>
        /// <returns>Less than zero: This node as the path score less than other node or the path score equal and the current cost less than other node.
        /// Zero: This node as the path score  and the current cost equal to other node. Greater than zero:
        /// This node as the path score greater than other node or the current cost greater and the path score equal than other node.</returns>
        public int CompareTo(Node other)
        {
            int FCompare = this.F_PathScore.CompareTo(other.F_PathScore);

            if (FCompare == 0)
            {
                // Path scores equals: return the current scores comparison
                return this.G_CurrentCost.CompareTo(other.G_CurrentCost);
            }
            else
            {
                // Path scores not equals: return the path scores comparison
                return FCompare;
            }
        }

        #endregion

        #region IEquatable<Node> Members

        /// <summary>
        /// Return a value indicating whether this istance and a specific node object rappresent the same node
        /// </summary>
        /// <param name="other">Other node istance</param>
        /// <returns>True if this and the other istance rappresent the same node</returns>
        public bool Equals(Node other)
        {
            return this.NodeGuid.Equals(other.NodeGuid);
        }

        #endregion
    }

    /// <summary>
    /// Implement function to find a path with the A* algorithm
    /// </summary>
    public class PathFinder
    {
        #region Private data

        /// <summary>
        /// Search state enumeration used to manage the search function
        /// </summary>
        private enum SearchState
        {
            Searching = 0,
            Found = 1,
            NotFound = 2
        }

        /// <summary>
        /// Structure used to store the find thread parameters
        /// </summary>
        private struct AsynchFindPathParams
        {
            public Guid ThreadGuid;
            public PathFoundEventHandler PathFoundEventDelegate;
        }

        /// <summary>
        /// Contains the lists of nodes that could be visited or must be ignored
        /// </summary>
        private class OpenClosedNodeCollection
        {
            #region Private data

            private Dictionary<Guid, Node> OpenNodeDictionary;     // List of nodes that could be visited
            private Dictionary<Guid, Node> ClosedNodeDictionary;   // List of nodes that must not be considered

            #endregion

            #region Properties

            /// <summary>
            /// Get the nearest node to the destination
            /// </summary>
            public Node NearestNode
            {
                get
                {
                    Node[] NodeArray = new Node[this.OpenNodeDictionary.Count];
                    Node NearestNodeInArray = null;

                    // Continue only if there are nodes in the dictionary
                    if (NodeArray.Length > 0)
                    {
                        this.OpenNodeDictionary.Values.CopyTo(NodeArray, 0);

                        // Start the search from the first node
                        NearestNodeInArray = NodeArray[0];

                        for (int i = 0; i < NodeArray.Length; i++)
                        {
                            // If there is a node lesser than the current nearest node, this must be the new nearest
                            if (NearestNodeInArray.CompareTo(NodeArray[i]) > 0)
                            {
                                NearestNodeInArray = NodeArray[i];
                            }
                        }
                    }

                    return NearestNodeInArray;
                }
            }

            #endregion

            #region Costrunctor

            /// <summary>
            /// Create a new instance of the OpenClosedNodeCollection
            /// </summary>
            public OpenClosedNodeCollection()
            {
                this.OpenNodeDictionary = new Dictionary<Guid,Node>();
                this.ClosedNodeDictionary = new Dictionary<Guid, Node>();
            }

            #endregion

            #region Public methods

            /// <summary>
            /// Add a node to the open list (nodes that could be visited)
            /// </summary>
            /// <param name="node">Node to add to the open list</param>
            public void AddOpen(Node node)
            {
                // Skip closed nodes
                if (!this.ClosedNodeDictionary.ContainsKey(node.NodeGuid))
                {
                    Node OpenNode = null;

                    // Verifyng in the current path its better than previous
                    if (this.OpenNodeDictionary.TryGetValue(node.NodeGuid, out OpenNode))
                    {
                        if (OpenNode.G_CurrentCost >= node.G_CurrentCost)
                        {
                            // Current it's better or equal, substitute the node
                            this.OpenNodeDictionary[node.NodeGuid] = node;
                        }
                    }
                    else
                    {
                        // No preovious path, adding the node
                        this.OpenNodeDictionary.Add(node.NodeGuid, node);
                    }
                }
            }

            /// <summary>
            /// Add a node to the closed list (nodes that must not be considered)
            /// </summary>
            /// <param name="node">Node to add to the close list</param>
            public void AddClosed(Node node)
            {
                // Add the node to the close dictionary and remove it, if exist,
                // from the open dictionary
                if (!this.ClosedNodeDictionary.ContainsKey(node.NodeGuid))
                {
                    this.ClosedNodeDictionary.Add(node.NodeGuid, node);
                    this.OpenNodeDictionary.Remove(node.NodeGuid);
                }
            }

            #endregion
        }

        /// <summary>
        /// Dictionary used to manage the open threads for multi-threading
        /// </summary>
        private Dictionary<Guid, Thread> ThreadDictionary;

        #endregion

        #region Events

        /// <summary>
        /// Occurs when the asynch path finding function return
        /// </summary>
        public event PathFoundEventHandler PathFound;

        #endregion

        #region Properties

        /// <summary>
        /// Node map where to find the path
        /// </summary>
        public readonly INodeMap NodeMap;

        #endregion

        #region Constructors

        /// <summary>
        /// Create a new istance of PathFinder
        /// </summary>
        /// <param name="nodeMap">Node map where to find a path</param>
        public PathFinder(INodeMap nodeMap)
        {
            this.ThreadDictionary = new Dictionary<Guid, Thread>();
            this.NodeMap = nodeMap;
        }

        #endregion

        #region Private methods

        /// <summary>
        /// Function working as a new thread used to launch dhe FindPath method asynchronously
        /// </summary>
        /// <param name="param">AsyncFindPathParams casted to object</param>
        private void DoAsynchFindPath(object param)
        {
            AsynchFindPathParams Params = (AsynchFindPathParams)param;
            Node[] Path = this.FindPath();

            Params.PathFoundEventDelegate.Invoke(Path);

            this.ThreadDictionary.Remove(Params.ThreadGuid);
        }

        #endregion

        #region Public methods

        /// <summary>
        /// Find a path
        /// </summary>
        /// <returns>Node array that define the path</returns>
        public Node[] FindPath()
        {
            List<Node> PathNodeList = new List<Node>();                 // List of node that defines the path
            Node ActualNode = this.NodeMap.GetStartingNode();           // Actual visited node (beginning from starting node)
            Node DestinationNode = this.NodeMap.GetDestinationNode();   // Destination node to be reached

            // Return no path if there are no starting or destination node
            if (ActualNode == null || DestinationNode == null)
            {
                return PathNodeList.ToArray();
            }

            Node[] ChildNodes = null;                                                   // Array of child nodes
            Node NearestNode = null;                                                    // Node nearest to the actual node
            SearchState SearchResult = SearchState.Searching;                           // Flag used to let the function cycle
            OpenClosedNodeCollection NodeCollection = new OpenClosedNodeCollection();   // Collection containing the nodes that can be visited or must be ignored

            // Begin the search
            do
            {
                // Get the child nodes 
                ChildNodes = this.NodeMap.GetChildNodes(ActualNode);

                // Adding actual node to the closed list
                NodeCollection.AddClosed(ActualNode);

                // Analizing child nodes
                if (ChildNodes.Length > 0)
                {
                    foreach (Node node in ChildNodes)
                    {
                        // Add child node the the open list
                        NodeCollection.AddOpen(node);
                    }

                    // Getting the nearest node
                    NearestNode = NodeCollection.NearestNode;

                    if (NearestNode != null)
                    {
                        ActualNode = NearestNode;

                        // Destination reached, stopping the search
                        if (ActualNode.Equals(DestinationNode))
                        {
                            SearchResult = SearchState.Found;
                        }
                    }
                    else
                    {
                        // No open nodes, going backward
                        ActualNode = ActualNode.Parent;

                        // No path avalaible, stopping the search
                        if (ActualNode == null)
                        {
                            SearchResult = SearchState.NotFound;
                        }
                    }
                }
                else
                {
                    // No child nodes, going backward
                    ActualNode = ActualNode.Parent;

                    // No path avalaible, stopping the search
                    if (ActualNode == null)
                    {
                        SearchResult = SearchState.NotFound;
                    }
                }
            }
            while (SearchResult == SearchState.Searching);

            // Create the path if destination as been reached
            if (SearchResult == SearchState.Found)
            {
                // Pupulate the list. Starting node must be the first in the list
                while (ActualNode != null)
                {
                    PathNodeList.Insert(0, ActualNode);
                    ActualNode = ActualNode.Parent;
                }
            }

            // Return an array so the list cannot be sorted
            return PathNodeList.ToArray();
        }

        #region Async functions

        /// <summary>
        /// Find a path asynchronously
        /// </summary>
        /// <returns>Async find istance guid</returns>
        public Guid AsynchFindPath()
        {
            return this.AsynchFindPath(this.PathFound);
        }

        /// <summary>
        /// Find a path asynchronously
        /// </summary>
        /// <param name="pathFoundEventDelegate">Event that occurs when the asynch path finding function return</param>
        /// <returns>Async find istance guid</returns>
        public Guid AsynchFindPath(PathFoundEventHandler pathFoundEventDelegate)
        {
            Guid ThreadGuid = Guid.Empty;

            if (pathFoundEventDelegate == null)
            {
                throw new ArgumentNullException();
            }
            else
            {
                Thread FindThread = new Thread(new ParameterizedThreadStart(this.DoAsynchFindPath));
                AsynchFindPathParams Params = new AsynchFindPathParams();
                ThreadGuid = Guid.NewGuid();

                this.ThreadDictionary.Add(ThreadGuid, FindThread);

                Params.ThreadGuid = ThreadGuid;
                Params.PathFoundEventDelegate = pathFoundEventDelegate;

                FindThread.Start(Params);
            }

            return ThreadGuid;
        }

        /// <summary>
        /// Stop a specific asynch find istance
        /// </summary>
        /// <param name="findIstanceGuid">Asynch find istance guid</param>
        public void StopAsynchFind(Guid findIstanceGuid)
        {
            if (this.ThreadDictionary.ContainsKey(findIstanceGuid))
            {
                this.ThreadDictionary[findIstanceGuid].Abort();
                this.ThreadDictionary.Remove(findIstanceGuid);
            }
        }

        /// <summary>
        /// Stop all asynch find istances
        /// </summary>
        public void StopAllAsynchFind()
        {
            foreach (Thread thread in this.ThreadDictionary.Values)
            {
                thread.Abort();
            }

            this.ThreadDictionary.Clear();
        }

        #endregion

        #endregion
    }

    /// <summary>
    /// Implement a set of functions to navigate a path
    /// </summary>
    public class Navigator
    {
        #region Events

        /// <summary>
        /// Occurs when entered in a node
        /// </summary>
        public event NodeNavigationEventHandler NodeEnter;

        /// <summary>
        /// Occurs when leaved a node
        /// </summary>
        public event NodeNavigationEventHandler NodeLeave;

        /// <summary>
        /// Occurs when returned to the beginning of the path
        /// </summary>
        public event NodeNavigationEventHandler ReturnedToBeginning;

        /// <summary>
        /// Occurs when reached the end of the path
        /// </summary>
        public event NodeNavigationEventHandler EndReached;

        #endregion

        #region Properties

        /// <summary>
        /// Get the node array that define the path
        /// </summary>
        public readonly Node[] Path;

        /// <summary>
        /// Get the index of the actual node
        /// </summary>
        public int Index
        {
            get;
            private set;
        }

        /// <summary>
        /// Get the lenght of the path
        /// </summary>
        public int PathLenght
        {
            get
            {
                return this.Path.Length;
            }
        }

        #endregion

        #region Constructors

        /// <summary>
        /// Create a new Navigator instance
        /// </summary>
        /// <param name="Path">Node path to navigate</param>
        public Navigator(Node[] Path)
        {
            this.Path = Path;
            this.Index = 0;

            this.NodeEnter = null;
            this.NodeLeave = null;
            this.ReturnedToBeginning = null;
            this.EndReached = null;
        }

        #endregion

        #region Public methods

        /// <summary>
        /// Move forward in the path
        /// </summary>
        /// <returns>True if the movement was succesfull done</returns>
        public bool GoForward()
        {
            return this.GoToIndex(this.Index - 1);
        }

        /// <summary>
        /// Move backward in the path
        /// </summary>
        /// <returns>True if the movement was succesfull done</returns>
        public bool GoNext()
        {
            return this.GoToIndex(this.Index + 1);
        }

        /// <summary>
        /// Go to a specific position in the path
        /// </summary>
        /// <param name="index">Index of the position</param>
        /// <returns>True if the movement was succesfull done</returns>
        public bool GoToIndex(int index)
        {
            if (index < 0 || index > this.PathLenght - 1)
            {
                return false;
            }

            if (this.NodeLeave != null)
            {
                NodeNavigationEventArgs NodeLeaveEventArgs = new NodeNavigationEventArgs(this.Path[this.Index], this.Path.Length, this.Index);
                this.NodeLeave.Invoke(this, NodeLeaveEventArgs);
            }

            this.Index = index;

            if (this.NodeEnter != null)
            {
                NodeNavigationEventArgs NodeEnterEventArgs = new NodeNavigationEventArgs(this.Path[this.Index], this.Path.Length, this.Index);
                this.NodeEnter.Invoke(this, NodeEnterEventArgs);
            }

            if (this.Index == 0 && this.ReturnedToBeginning != null)
            {
                NodeNavigationEventArgs NodeBeginningEventArgs = new NodeNavigationEventArgs(this.Path[this.Index], this.Path.Length, this.Index);
                this.EndReached.Invoke(this, NodeBeginningEventArgs);
            }

            if (this.Index == this.PathLenght - 1 && this.EndReached != null)
            {
                NodeNavigationEventArgs NodeEndEventArgs = new NodeNavigationEventArgs(this.Path[this.Index], this.Path.Length, this.Index);
                this.EndReached.Invoke(this, NodeEndEventArgs);
            }

            return true;
        }

        /// <summary>
        /// Go to the end of the path
        /// </summary>
        /// <returns>True if the movement was succesfull done</returns>
        public bool GoToEnd()
        {
            return this.GoToIndex(this.PathLenght - 1);
        }

        /// <summary>
        /// Go to the beginning of the path
        /// </summary>
        /// <returns>True if the movement was succesfull done</returns>
        public bool GoToBeginning()
        {
            return this.GoToIndex(0);
        }

        #endregion
    }
}
